#include <opencv2/opencv.hpp>
#include <opencv2/tracking.hpp>
#include <opencv2/core/ocl.hpp>
#include <iostream>
#include <chrono>
#include <mutex>
#include <thread>
#include <vector>
#include "UDPimg.h"

using namespace cv;
using namespace std;

typedef struct {
    int x1, x2, y1, y2;
}Rec;

static Rec waiting_rec;
static bool changed = false;
static std::mutex mtx1;

void receiveRecUDP(int port){
    while(1){
        struct sockaddr_in serverAddr, clientAddr;
        int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd < 0) {
            std::cerr << "Socket creation failed" << std::endl;
            exit(1);
        }

        memset(&serverAddr, 0, sizeof(serverAddr));
        serverAddr.sin_family = AF_INET;
        serverAddr.sin_addr.s_addr = INADDR_ANY;
        serverAddr.sin_port = htons(port);

        if (bind(sockfd, (const struct sockaddr *)&serverAddr, sizeof(serverAddr)) < 0) {
            std::cerr << "Bind failed" << std::endl;
            close(sockfd);
            exit(1);
        }
        socklen_t len = sizeof(clientAddr);

        printf("Lisenting.....\n");
        Rec buf;
        recvfrom(sockfd, &buf, sizeof(buf), 0, (struct sockaddr*)&clientAddr, &len);

        {
            std::lock_guard<std::mutex> lock(mtx1);
            changed = true;
            waiting_rec = buf;
        }

        printf("Received: x1: %d, y1: %d, x2: %d, y2: %d\n", waiting_rec.x1, waiting_rec.y1, waiting_rec.x2, waiting_rec.y2);
        close(sockfd);
    }
}

int main() {
    char device_name[] = "/dev/video0";
    cv::VideoCapture cap("/mnt/tfcard/bag.mp4");

    if(!cap.isOpened()){
        printf("Open failed.\n");
        return 0;
    }

    int frame_width = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_WIDTH));
    int frame_height = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_HEIGHT));
    double fps = cap.get(cv::CAP_PROP_FPS);
    int fourcc = static_cast<int>(cap.get(cv::CAP_PROP_FOURCC));
    printf("F_width: %d, F_height: %d, F_fps: %lf\n", frame_width, frame_height, fps);

    // const char* serverIP = "192.168.200.100";
    // int serverPort = 65500, cnt = 0;
    typename std::chrono::high_resolution_clock::time_point last_time;
    // double fps;
    // bool first = true;

    // std::vector<std::thread> rev_threads;
    // rev_threads.emplace_back(receiveRecUDP, 65400);

    cv::VideoWriter output("/mnt/tfcard/output.mp4", fourcc, fps, cv::Size(frame_width, frame_height));

    Rect bbox(311, 140, 121, 115); // (311, 140) (432, 255)
    printf("bbox.x: %d, bbox.y: %d\n", bbox.x, bbox.y);
    Ptr<Tracker> tracker = TrackerKCF::create();
    Mat frame;
    bool first = true;
    int cnt = 0;

    while(1){
        cap >> frame;
        if(frame.empty()){
            printf("Frame == NULL\n");
            break;
        }
        if(first){
            first = !first;
            tracker->init(frame, bbox);
        }
        tracker->update(frame, bbox);
        printf("bboxX1 %d, bboxY1 %d, bboxX2 %d, bboxY2 %d\n", bbox.x, bbox.y, bbox.width + bbox.x, bbox.height + bbox.y);
        rectangle( frame, bbox, Scalar( 255, 0, 0 ), 2, 1 );
        // sendImageUDP(frame, serverIP, serverPort);
        // nanosleep(&ts, &rem);

        auto now = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time).count();
        cnt++;
        if(duration >= 1000){
            last_time = now;
            fps = cnt * 1000.0 / duration;
            printf("%lf FPS.\n", fps);
            cnt = 0;
        }
        output.write(frame);
    }

return 0;
}